#include <math.h>
#include <string.h>
#include "pm_std.h"
#include "sm_std.h"
#include "ne_std.h"
#include "ne_dae.h"
#include "sm_ssci_run_time_errors.h"
#include "sm_RuntimeDerivedValuesBundle.h"
#include "sm_CTarget.h"
void openManipulatorTrajectoryPlanning_6a081443_1_setTargets ( const
RuntimeDerivedValuesBundle * rtdv , CTarget * targets ) { ( void ) rtdv ; (
void ) targets ; } void
openManipulatorTrajectoryPlanning_6a081443_1_resetAsmStateVector ( const void
* mech , double * state ) { double xx [ 1 ] ; ( void ) mech ; xx [ 0 ] = 0.0
; state [ 0 ] = xx [ 0 ] ; state [ 1 ] = xx [ 0 ] ; state [ 2 ] = xx [ 0 ] ;
state [ 3 ] = xx [ 0 ] ; state [ 4 ] = xx [ 0 ] ; state [ 5 ] = xx [ 0 ] ;
state [ 6 ] = xx [ 0 ] ; state [ 7 ] = xx [ 0 ] ; state [ 8 ] = xx [ 0 ] ;
state [ 9 ] = xx [ 0 ] ; state [ 10 ] = xx [ 0 ] ; state [ 11 ] = xx [ 0 ] ;
} void
openManipulatorTrajectoryPlanning_6a081443_1_initializeTrackedAngleState (
const void * mech , const RuntimeDerivedValuesBundle * rtdv , const int *
modeVector , const double * motionData , double * state ) { const double *
rtdvd = rtdv -> mDoubles . mValues ; const int * rtdvi = rtdv -> mInts .
mValues ; ( void ) mech ; ( void ) rtdvd ; ( void ) rtdvi ; ( void ) state ;
( void ) modeVector ; ( void ) motionData ; } void
openManipulatorTrajectoryPlanning_6a081443_1_computeDiscreteState ( const
void * mech , const RuntimeDerivedValuesBundle * rtdv , double * state ) {
const double * rtdvd = rtdv -> mDoubles . mValues ; const int * rtdvi = rtdv
-> mInts . mValues ; ( void ) mech ; ( void ) rtdvd ; ( void ) rtdvi ; ( void
) state ; } void openManipulatorTrajectoryPlanning_6a081443_1_adjustPosition
( const void * mech , const double * dofDeltas , double * state ) { ( void )
mech ; state [ 0 ] = state [ 0 ] + dofDeltas [ 0 ] ; state [ 2 ] = state [ 2
] + dofDeltas [ 1 ] ; state [ 4 ] = state [ 4 ] + dofDeltas [ 2 ] ; state [ 6
] = state [ 6 ] + dofDeltas [ 3 ] ; state [ 8 ] = state [ 8 ] + dofDeltas [ 4
] ; state [ 10 ] = state [ 10 ] + dofDeltas [ 5 ] ; } static void
perturbAsmJointPrimitiveState_0_0 ( double mag , double * state ) { state [ 0
] = state [ 0 ] + mag ; } static void perturbAsmJointPrimitiveState_0_0v (
double mag , double * state ) { state [ 0 ] = state [ 0 ] + mag ; state [ 1 ]
= state [ 1 ] - 0.875 * mag ; } static void perturbAsmJointPrimitiveState_1_0
( double mag , double * state ) { state [ 2 ] = state [ 2 ] + mag ; } static
void perturbAsmJointPrimitiveState_1_0v ( double mag , double * state ) {
state [ 2 ] = state [ 2 ] + mag ; state [ 3 ] = state [ 3 ] - 0.875 * mag ; }
static void perturbAsmJointPrimitiveState_2_0 ( double mag , double * state )
{ state [ 4 ] = state [ 4 ] + mag ; } static void
perturbAsmJointPrimitiveState_2_0v ( double mag , double * state ) { state [
4 ] = state [ 4 ] + mag ; state [ 5 ] = state [ 5 ] - 0.875 * mag ; } static
void perturbAsmJointPrimitiveState_3_0 ( double mag , double * state ) {
state [ 6 ] = state [ 6 ] + mag ; } static void
perturbAsmJointPrimitiveState_3_0v ( double mag , double * state ) { state [
6 ] = state [ 6 ] + mag ; state [ 7 ] = state [ 7 ] - 0.875 * mag ; } static
void perturbAsmJointPrimitiveState_4_0 ( double mag , double * state ) {
state [ 8 ] = state [ 8 ] + mag ; } static void
perturbAsmJointPrimitiveState_4_0v ( double mag , double * state ) { state [
8 ] = state [ 8 ] + mag ; state [ 9 ] = state [ 9 ] - 0.875 * mag ; } static
void perturbAsmJointPrimitiveState_5_0 ( double mag , double * state ) {
state [ 10 ] = state [ 10 ] + mag ; } static void
perturbAsmJointPrimitiveState_5_0v ( double mag , double * state ) { state [
10 ] = state [ 10 ] + mag ; state [ 11 ] = state [ 11 ] - 0.875 * mag ; }
void
openManipulatorTrajectoryPlanning_6a081443_1_perturbAsmJointPrimitiveState (
const void * mech , size_t stageIdx , size_t primIdx , double mag , boolean_T
doPerturbVelocity , double * state ) { ( void ) mech ; ( void ) stageIdx ; (
void ) primIdx ; ( void ) mag ; ( void ) doPerturbVelocity ; ( void ) state ;
switch ( ( stageIdx * 6 + primIdx ) * 2 + ( doPerturbVelocity ? 1 : 0 ) ) {
case 0 : perturbAsmJointPrimitiveState_0_0 ( mag , state ) ; break ; case 1 :
perturbAsmJointPrimitiveState_0_0v ( mag , state ) ; break ; case 12 :
perturbAsmJointPrimitiveState_1_0 ( mag , state ) ; break ; case 13 :
perturbAsmJointPrimitiveState_1_0v ( mag , state ) ; break ; case 24 :
perturbAsmJointPrimitiveState_2_0 ( mag , state ) ; break ; case 25 :
perturbAsmJointPrimitiveState_2_0v ( mag , state ) ; break ; case 36 :
perturbAsmJointPrimitiveState_3_0 ( mag , state ) ; break ; case 37 :
perturbAsmJointPrimitiveState_3_0v ( mag , state ) ; break ; case 48 :
perturbAsmJointPrimitiveState_4_0 ( mag , state ) ; break ; case 49 :
perturbAsmJointPrimitiveState_4_0v ( mag , state ) ; break ; case 60 :
perturbAsmJointPrimitiveState_5_0 ( mag , state ) ; break ; case 61 :
perturbAsmJointPrimitiveState_5_0v ( mag , state ) ; break ; } } void
openManipulatorTrajectoryPlanning_6a081443_1_computePosDofBlendMatrix ( const
void * mech , size_t stageIdx , size_t primIdx , const double * state , int
partialType , double * matrix ) { ( void ) mech ; ( void ) stageIdx ; ( void
) primIdx ; ( void ) state ; ( void ) partialType ; ( void ) matrix ; switch
( ( stageIdx * 6 + primIdx ) ) { } } void
openManipulatorTrajectoryPlanning_6a081443_1_computeVelDofBlendMatrix ( const
void * mech , size_t stageIdx , size_t primIdx , const double * state , int
partialType , double * matrix ) { ( void ) mech ; ( void ) stageIdx ; ( void
) primIdx ; ( void ) state ; ( void ) partialType ; ( void ) matrix ; switch
( ( stageIdx * 6 + primIdx ) ) { } } void
openManipulatorTrajectoryPlanning_6a081443_1_projectPartiallyTargetedPos (
const void * mech , size_t stageIdx , size_t primIdx , const double *
origState , int partialType , double * state ) { ( void ) mech ; ( void )
stageIdx ; ( void ) primIdx ; ( void ) origState ; ( void ) partialType ; (
void ) state ; switch ( ( stageIdx * 6 + primIdx ) ) { } } void
openManipulatorTrajectoryPlanning_6a081443_1_propagateMotion ( const void *
mech , const RuntimeDerivedValuesBundle * rtdv , const double * state ,
double * motionData ) { const double * rtdvd = rtdv -> mDoubles . mValues ;
const int * rtdvi = rtdv -> mInts . mValues ; double xx [ 120 ] ; ( void )
mech ; ( void ) rtdvd ; ( void ) rtdvi ; xx [ 0 ] = - 0.9999793598170716 ; xx
[ 1 ] = 5.294470415447712e-3 ; xx [ 2 ] = 2.177120480925496e-3 ; xx [ 3 ] =
2.916962336305661e-3 ; xx [ 4 ] = 0.5 ; xx [ 5 ] = xx [ 4 ] * state [ 0 ] ;
xx [ 6 ] = 4.385038631106116e-3 ; xx [ 7 ] = sin ( xx [ 5 ] ) ; xx [ 8 ] =
0.01057602111633074 ; xx [ 9 ] = 0.999934457458863 ; xx [ 10 ] = cos ( xx [ 5
] ) ; xx [ 11 ] = xx [ 6 ] * xx [ 7 ] ; xx [ 12 ] = - ( xx [ 8 ] * xx [ 7 ] )
; xx [ 13 ] = xx [ 9 ] * xx [ 7 ] ; pm_math_Quaternion_compose_ra ( xx + 0 ,
xx + 10 , xx + 14 ) ; xx [ 0 ] = 0.012 ; xx [ 1 ] = 0.0 ; xx [ 2 ] = 0.017 ;
xx [ 10 ] = - 0.9984344664497182 ; xx [ 11 ] = - 2.745981335367077e-3 ; xx [
12 ] = - 0.05574004796882191 ; xx [ 13 ] = 3.758037281069619e-3 ; xx [ 3 ] =
xx [ 4 ] * state [ 2 ] ; xx [ 5 ] = 0.01415235514121426 ; xx [ 7 ] = sin ( xx
[ 3 ] ) ; xx [ 18 ] = 0.9998919020591995 ; xx [ 19 ] = 3.986858461632603e-3 ;
xx [ 20 ] = cos ( xx [ 3 ] ) ; xx [ 21 ] = - ( xx [ 5 ] * xx [ 7 ] ) ; xx [
22 ] = xx [ 18 ] * xx [ 7 ] ; xx [ 23 ] = xx [ 19 ] * xx [ 7 ] ;
pm_math_Quaternion_compose_ra ( xx + 10 , xx + 20 , xx + 24 ) ; xx [ 3 ] =
2.543322406041548e-4 ; xx [ 7 ] = - 6.134092247471828e-4 ; xx [ 10 ] =
0.05799619853261406 ; xx [ 20 ] = - 0.9984606559510578 ; xx [ 21 ] = -
1.525595350473025e-3 ; xx [ 22 ] = 0.05467563387767577 ; xx [ 23 ] = -
9.195984813180454e-3 ; xx [ 11 ] = xx [ 4 ] * state [ 4 ] ; xx [ 12 ] =
4.564927305666077e-3 ; xx [ 13 ] = sin ( xx [ 11 ] ) ; xx [ 28 ] =
0.9999885204623049 ; xx [ 29 ] = 1.456153255698589e-3 ; xx [ 30 ] = cos ( xx
[ 11 ] ) ; xx [ 31 ] = xx [ 12 ] * xx [ 13 ] ; xx [ 32 ] = xx [ 28 ] * xx [
13 ] ; xx [ 33 ] = xx [ 29 ] * xx [ 13 ] ; pm_math_Quaternion_compose_ra ( xx
+ 20 , xx + 30 , xx + 34 ) ; xx [ 11 ] = 0.01017880053681747 ; xx [ 13 ] = -
3.736067231021393e-4 ; xx [ 20 ] = 0.1298316311137164 ; xx [ 30 ] = -
0.9953936295809525 ; xx [ 31 ] = - 5.058992580434195e-4 ; xx [ 32 ] =
0.09584258966610114 ; xx [ 33 ] = 2.337576029516223e-3 ; xx [ 21 ] = xx [ 4 ]
* state [ 6 ] ; xx [ 4 ] = 8.310758752905212e-6 ; xx [ 22 ] = sin ( xx [ 21 ]
) ; xx [ 23 ] = 0.9999999999652471 ; xx [ 38 ] = 6.610584974975353e-7 ; xx [
39 ] = cos ( xx [ 21 ] ) ; xx [ 40 ] = xx [ 4 ] * xx [ 22 ] ; xx [ 41 ] = xx
[ 23 ] * xx [ 22 ] ; xx [ 42 ] = - ( xx [ 38 ] * xx [ 22 ] ) ;
pm_math_Quaternion_compose_ra ( xx + 30 , xx + 39 , xx + 43 ) ; xx [ 21 ] =
0.1239983924299681 ; xx [ 22 ] = - 5.656415752171374e-4 ; xx [ 30 ] = -
2.805786735064276e-4 ; xx [ 31 ] = - 0.9952875672845549 ; xx [ 32 ] = -
7.396429119177483e-8 ; xx [ 33 ] = - 0.09696730578610657 ; xx [ 39 ] =
4.167847961603077e-6 ; xx [ 40 ] = 0.05259203501857801 ; xx [ 41 ] = xx [ 40
] - 8.310758753113379e-6 * state [ 8 ] ; xx [ 42 ] = 4.439189670580984e-7 ;
xx [ 47 ] = - ( xx [ 42 ] + xx [ 23 ] * state [ 8 ] ) ; xx [ 48 ] =
0.01034590993618254 ; xx [ 49 ] = 6.610584966093569e-7 * state [ 8 ] - xx [
48 ] ; xx [ 50 ] = - 7.396429069217447e-8 ; xx [ 51 ] = 4.167847961547566e-6
; xx [ 52 ] = xx [ 40 ] + xx [ 4 ] * state [ 10 ] ; xx [ 40 ] = xx [ 23 ] *
state [ 10 ] - xx [ 42 ] ; xx [ 42 ] = - ( xx [ 48 ] + xx [ 38 ] * state [ 10
] ) ; pm_math_Quaternion_compose_ra ( xx + 14 , xx + 24 , xx + 53 ) ; xx [ 57
] = xx [ 3 ] ; xx [ 58 ] = xx [ 7 ] ; xx [ 59 ] = xx [ 10 ] ;
pm_math_Quaternion_xform_ra ( xx + 14 , xx + 57 , xx + 60 ) ; xx [ 48 ] = xx
[ 60 ] + xx [ 0 ] ; xx [ 60 ] = xx [ 62 ] + xx [ 2 ] ;
pm_math_Quaternion_compose_ra ( xx + 53 , xx + 34 , xx + 62 ) ; xx [ 66 ] =
xx [ 11 ] ; xx [ 67 ] = xx [ 13 ] ; xx [ 68 ] = xx [ 20 ] ;
pm_math_Quaternion_xform_ra ( xx + 53 , xx + 66 , xx + 69 ) ; xx [ 72 ] = xx
[ 69 ] + xx [ 48 ] ; xx [ 73 ] = xx [ 70 ] + xx [ 61 ] ; xx [ 69 ] = xx [ 71
] + xx [ 60 ] ; pm_math_Quaternion_compose_ra ( xx + 62 , xx + 43 , xx + 74 )
; xx [ 78 ] = xx [ 21 ] ; xx [ 79 ] = xx [ 22 ] ; xx [ 80 ] = xx [ 30 ] ;
pm_math_Quaternion_xform_ra ( xx + 62 , xx + 78 , xx + 81 ) ; xx [ 70 ] = xx
[ 81 ] + xx [ 72 ] ; xx [ 71 ] = xx [ 82 ] + xx [ 73 ] ; xx [ 81 ] = xx [ 83
] + xx [ 69 ] ; xx [ 82 ] = xx [ 31 ] ; xx [ 83 ] = xx [ 50 ] ; xx [ 84 ] =
xx [ 33 ] ; xx [ 85 ] = xx [ 51 ] ; pm_math_Quaternion_compose_ra ( xx + 74 ,
xx + 82 , xx + 86 ) ; xx [ 90 ] = xx [ 52 ] ; xx [ 91 ] = xx [ 40 ] ; xx [ 92
] = xx [ 42 ] ; pm_math_Quaternion_xform_ra ( xx + 74 , xx + 90 , xx + 93 ) ;
xx [ 96 ] = xx [ 31 ] ; xx [ 97 ] = xx [ 32 ] ; xx [ 98 ] = xx [ 33 ] ; xx [
99 ] = xx [ 39 ] ; pm_math_Quaternion_compose_ra ( xx + 74 , xx + 96 , xx +
100 ) ; xx [ 104 ] = xx [ 41 ] ; xx [ 105 ] = xx [ 47 ] ; xx [ 106 ] = xx [
49 ] ; pm_math_Quaternion_xform_ra ( xx + 74 , xx + 104 , xx + 107 ) ; xx [
110 ] = xx [ 6 ] * state [ 1 ] ; xx [ 6 ] = - ( xx [ 8 ] * state [ 1 ] ) ; xx
[ 8 ] = xx [ 9 ] * state [ 1 ] ; xx [ 111 ] = xx [ 110 ] ; xx [ 112 ] = xx [
6 ] ; xx [ 113 ] = xx [ 8 ] ; pm_math_Quaternion_inverseXform_ra ( xx + 24 ,
xx + 111 , xx + 114 ) ; xx [ 9 ] = xx [ 114 ] - xx [ 5 ] * state [ 3 ] ; xx [
5 ] = xx [ 115 ] + xx [ 18 ] * state [ 3 ] ; xx [ 18 ] = xx [ 116 ] + xx [ 19
] * state [ 3 ] ; pm_math_Vector3_cross_ra ( xx + 111 , xx + 57 , xx + 114 )
; pm_math_Quaternion_inverseXform_ra ( xx + 24 , xx + 114 , xx + 57 ) ; xx [
111 ] = xx [ 9 ] ; xx [ 112 ] = xx [ 5 ] ; xx [ 113 ] = xx [ 18 ] ;
pm_math_Quaternion_inverseXform_ra ( xx + 34 , xx + 111 , xx + 114 ) ; xx [
19 ] = xx [ 114 ] + xx [ 12 ] * state [ 5 ] ; xx [ 12 ] = xx [ 115 ] + xx [
28 ] * state [ 5 ] ; xx [ 28 ] = xx [ 116 ] + xx [ 29 ] * state [ 5 ] ;
pm_math_Vector3_cross_ra ( xx + 111 , xx + 66 , xx + 114 ) ; xx [ 66 ] = xx [
114 ] + xx [ 57 ] ; xx [ 67 ] = xx [ 115 ] + xx [ 58 ] ; xx [ 68 ] = xx [ 116
] + xx [ 59 ] ; pm_math_Quaternion_inverseXform_ra ( xx + 34 , xx + 66 , xx +
111 ) ; xx [ 66 ] = xx [ 19 ] ; xx [ 67 ] = xx [ 12 ] ; xx [ 68 ] = xx [ 28 ]
; pm_math_Quaternion_inverseXform_ra ( xx + 43 , xx + 66 , xx + 114 ) ; xx [
29 ] = xx [ 114 ] + xx [ 4 ] * state [ 7 ] ; xx [ 4 ] = xx [ 115 ] + xx [ 23
] * state [ 7 ] ; xx [ 23 ] = xx [ 116 ] - xx [ 38 ] * state [ 7 ] ;
pm_math_Vector3_cross_ra ( xx + 66 , xx + 78 , xx + 114 ) ; xx [ 66 ] = xx [
114 ] + xx [ 111 ] ; xx [ 67 ] = xx [ 115 ] + xx [ 112 ] ; xx [ 68 ] = xx [
116 ] + xx [ 113 ] ; pm_math_Quaternion_inverseXform_ra ( xx + 43 , xx + 66 ,
xx + 78 ) ; xx [ 66 ] = xx [ 29 ] ; xx [ 67 ] = xx [ 4 ] ; xx [ 68 ] = xx [
23 ] ; pm_math_Quaternion_inverseXform_ra ( xx + 96 , xx + 66 , xx + 114 ) ;
pm_math_Vector3_cross_ra ( xx + 66 , xx + 104 , xx + 117 ) ; xx [ 104 ] = xx
[ 117 ] + xx [ 78 ] ; xx [ 105 ] = xx [ 118 ] + xx [ 79 ] ; xx [ 106 ] = xx [
119 ] + xx [ 80 ] ; pm_math_Quaternion_inverseXform_ra ( xx + 96 , xx + 104 ,
xx + 117 ) ; pm_math_Quaternion_inverseXform_ra ( xx + 82 , xx + 66 , xx + 96
) ; pm_math_Vector3_cross_ra ( xx + 66 , xx + 90 , xx + 104 ) ; xx [ 66 ] =
xx [ 104 ] + xx [ 78 ] ; xx [ 67 ] = xx [ 105 ] + xx [ 79 ] ; xx [ 68 ] = xx
[ 106 ] + xx [ 80 ] ; pm_math_Quaternion_inverseXform_ra ( xx + 82 , xx + 66
, xx + 90 ) ; motionData [ 0 ] = xx [ 14 ] ; motionData [ 1 ] = xx [ 15 ] ;
motionData [ 2 ] = xx [ 16 ] ; motionData [ 3 ] = xx [ 17 ] ; motionData [ 4
] = xx [ 0 ] ; motionData [ 5 ] = xx [ 1 ] ; motionData [ 6 ] = xx [ 2 ] ;
motionData [ 7 ] = xx [ 24 ] ; motionData [ 8 ] = xx [ 25 ] ; motionData [ 9
] = xx [ 26 ] ; motionData [ 10 ] = xx [ 27 ] ; motionData [ 11 ] = xx [ 3 ]
; motionData [ 12 ] = xx [ 7 ] ; motionData [ 13 ] = xx [ 10 ] ; motionData [
14 ] = xx [ 34 ] ; motionData [ 15 ] = xx [ 35 ] ; motionData [ 16 ] = xx [
36 ] ; motionData [ 17 ] = xx [ 37 ] ; motionData [ 18 ] = xx [ 11 ] ;
motionData [ 19 ] = xx [ 13 ] ; motionData [ 20 ] = xx [ 20 ] ; motionData [
21 ] = xx [ 43 ] ; motionData [ 22 ] = xx [ 44 ] ; motionData [ 23 ] = xx [
45 ] ; motionData [ 24 ] = xx [ 46 ] ; motionData [ 25 ] = xx [ 21 ] ;
motionData [ 26 ] = xx [ 22 ] ; motionData [ 27 ] = xx [ 30 ] ; motionData [
28 ] = xx [ 31 ] ; motionData [ 29 ] = xx [ 32 ] ; motionData [ 30 ] = xx [
33 ] ; motionData [ 31 ] = xx [ 39 ] ; motionData [ 32 ] = xx [ 41 ] ;
motionData [ 33 ] = xx [ 47 ] ; motionData [ 34 ] = xx [ 49 ] ; motionData [
35 ] = xx [ 31 ] ; motionData [ 36 ] = xx [ 50 ] ; motionData [ 37 ] = xx [
33 ] ; motionData [ 38 ] = xx [ 51 ] ; motionData [ 39 ] = xx [ 52 ] ;
motionData [ 40 ] = xx [ 40 ] ; motionData [ 41 ] = xx [ 42 ] ; motionData [
42 ] = xx [ 53 ] ; motionData [ 43 ] = xx [ 54 ] ; motionData [ 44 ] = xx [
55 ] ; motionData [ 45 ] = xx [ 56 ] ; motionData [ 46 ] = xx [ 48 ] ;
motionData [ 47 ] = xx [ 61 ] ; motionData [ 48 ] = xx [ 60 ] ; motionData [
49 ] = xx [ 62 ] ; motionData [ 50 ] = xx [ 63 ] ; motionData [ 51 ] = xx [
64 ] ; motionData [ 52 ] = xx [ 65 ] ; motionData [ 53 ] = xx [ 72 ] ;
motionData [ 54 ] = xx [ 73 ] ; motionData [ 55 ] = xx [ 69 ] ; motionData [
56 ] = xx [ 74 ] ; motionData [ 57 ] = xx [ 75 ] ; motionData [ 58 ] = xx [
76 ] ; motionData [ 59 ] = xx [ 77 ] ; motionData [ 60 ] = xx [ 70 ] ;
motionData [ 61 ] = xx [ 71 ] ; motionData [ 62 ] = xx [ 81 ] ; motionData [
63 ] = xx [ 86 ] ; motionData [ 64 ] = xx [ 87 ] ; motionData [ 65 ] = xx [
88 ] ; motionData [ 66 ] = xx [ 89 ] ; motionData [ 67 ] = xx [ 93 ] + xx [
70 ] ; motionData [ 68 ] = xx [ 94 ] + xx [ 71 ] ; motionData [ 69 ] = xx [
95 ] + xx [ 81 ] ; motionData [ 70 ] = xx [ 100 ] ; motionData [ 71 ] = xx [
101 ] ; motionData [ 72 ] = xx [ 102 ] ; motionData [ 73 ] = xx [ 103 ] ;
motionData [ 74 ] = xx [ 107 ] + xx [ 70 ] ; motionData [ 75 ] = xx [ 108 ] +
xx [ 71 ] ; motionData [ 76 ] = xx [ 109 ] + xx [ 81 ] ; motionData [ 77 ] =
xx [ 110 ] ; motionData [ 78 ] = xx [ 6 ] ; motionData [ 79 ] = xx [ 8 ] ;
motionData [ 80 ] = xx [ 1 ] ; motionData [ 81 ] = xx [ 1 ] ; motionData [ 82
] = xx [ 1 ] ; motionData [ 83 ] = xx [ 9 ] ; motionData [ 84 ] = xx [ 5 ] ;
motionData [ 85 ] = xx [ 18 ] ; motionData [ 86 ] = xx [ 57 ] ; motionData [
87 ] = xx [ 58 ] ; motionData [ 88 ] = xx [ 59 ] ; motionData [ 89 ] = xx [
19 ] ; motionData [ 90 ] = xx [ 12 ] ; motionData [ 91 ] = xx [ 28 ] ;
motionData [ 92 ] = xx [ 111 ] ; motionData [ 93 ] = xx [ 112 ] ; motionData
[ 94 ] = xx [ 113 ] ; motionData [ 95 ] = xx [ 29 ] ; motionData [ 96 ] = xx
[ 4 ] ; motionData [ 97 ] = xx [ 23 ] ; motionData [ 98 ] = xx [ 78 ] ;
motionData [ 99 ] = xx [ 79 ] ; motionData [ 100 ] = xx [ 80 ] ; motionData [
101 ] = xx [ 114 ] ; motionData [ 102 ] = xx [ 115 ] ; motionData [ 103 ] =
xx [ 116 ] ; motionData [ 104 ] = xx [ 117 ] ; motionData [ 105 ] = xx [ 118
] - state [ 9 ] ; motionData [ 106 ] = xx [ 119 ] ; motionData [ 107 ] = xx [
96 ] ; motionData [ 108 ] = xx [ 97 ] ; motionData [ 109 ] = xx [ 98 ] ;
motionData [ 110 ] = xx [ 90 ] ; motionData [ 111 ] = xx [ 91 ] + state [ 11
] ; motionData [ 112 ] = xx [ 92 ] ; } size_t
openManipulatorTrajectoryPlanning_6a081443_1_computeAssemblyError ( const
void * mech , const RuntimeDerivedValuesBundle * rtdv , size_t constraintIdx
, const int * modeVector , const double * motionData , double * error ) { (
void ) mech ; ( void ) rtdv ; ( void ) modeVector ; ( void ) motionData ; (
void ) error ; switch ( constraintIdx ) { } return 0 ; } size_t
openManipulatorTrajectoryPlanning_6a081443_1_computeAssemblyJacobian ( const
void * mech , const RuntimeDerivedValuesBundle * rtdv , size_t constraintIdx
, boolean_T forVelocitySatisfaction , const double * state , const int *
modeVector , const double * motionData , double * J ) { ( void ) mech ; (
void ) rtdv ; ( void ) state ; ( void ) modeVector ; ( void )
forVelocitySatisfaction ; ( void ) motionData ; ( void ) J ; switch (
constraintIdx ) { } return 0 ; } size_t
openManipulatorTrajectoryPlanning_6a081443_1_computeFullAssemblyJacobian (
const void * mech , const RuntimeDerivedValuesBundle * rtdv , const double *
state , const int * modeVector , const double * motionData , double * J ) {
const double * rtdvd = rtdv -> mDoubles . mValues ; const int * rtdvi = rtdv
-> mInts . mValues ; ( void ) mech ; ( void ) rtdvd ; ( void ) rtdvi ; ( void
) state ; ( void ) modeVector ; ( void ) motionData ; ( void ) J ; return 0 ;
} boolean_T
openManipulatorTrajectoryPlanning_6a081443_1_isInKinematicSingularity ( const
void * mech , const RuntimeDerivedValuesBundle * rtdv , size_t constraintIdx
, const int * modeVector , const double * motionData ) { ( void ) mech ; (
void ) rtdv ; ( void ) modeVector ; ( void ) motionData ; switch (
constraintIdx ) { } return 0 ; } void
openManipulatorTrajectoryPlanning_6a081443_1_convertStateVector ( const void
* asmMech , const RuntimeDerivedValuesBundle * rtdv , const void * simMech ,
const double * asmState , const int * asmModeVector , const int *
simModeVector , double * simState ) { const double * rtdvd = rtdv -> mDoubles
. mValues ; const int * rtdvi = rtdv -> mInts . mValues ; ( void ) asmMech ;
( void ) rtdvd ; ( void ) rtdvi ; ( void ) simMech ; ( void ) asmModeVector ;
( void ) simModeVector ; simState [ 0 ] = asmState [ 0 ] ; simState [ 1 ] =
asmState [ 1 ] ; simState [ 2 ] = asmState [ 2 ] ; simState [ 3 ] = asmState
[ 3 ] ; simState [ 4 ] = asmState [ 4 ] ; simState [ 5 ] = asmState [ 5 ] ;
simState [ 6 ] = asmState [ 6 ] ; simState [ 7 ] = asmState [ 7 ] ; simState
[ 8 ] = asmState [ 8 ] ; simState [ 9 ] = asmState [ 9 ] ; simState [ 10 ] =
asmState [ 10 ] ; simState [ 11 ] = asmState [ 11 ] ; }
